#include "motor.h"
#include <stdio.h>
#define MAX_VOLTAGE 14

Motor::Motor()
{
  char* serial = "SPEED";
       
  ftdi = new Serial(serial);

  for(int i = 1; i<=6; i++)
    getErrors(i);
}

void Motor::exitSafeStart(int index)
{
  unsigned char buffer[3] = {0xAA,index,0x03};
  ftdi->writeData(buffer, 3);
}

int Motor::getErrors(int index)
{

  unsigned char buffer[4] = {0xAA,index,0x21,0};
  int ret = 0;

  ftdi->writeData(&buffer[0],4);
  
  unsigned char returnBuffer[2] = {0,0};
  
  ftdi->readData(&returnBuffer[0],2);

  if((returnBuffer[0] & 0x01) == 1)
  {
    std::cout << "Safe start violation on motor " << index << std::endl;
    exitSafeStart(index);
    ret = -1;
  }
  
  if((returnBuffer[0] & 0x02) == 0x02)
  {
    std::cout << "Required channel invalid on motor " << index << std::endl;
    ret = -2;
  }
  
  if((returnBuffer[0] & 0x04) == 0x04)
  {
    std::cout << "Serial error on motor " << index << std::endl;
    unsigned char buffer2[4] = {0xAA,index,0x21,2};

    ftdi->writeData(buffer2,4);
  
    unsigned char returnBuffer2[2];
  
    ftdi->readData(&returnBuffer2[0],2);
    printf("%x%x\n",returnBuffer2[1],returnBuffer2[0]);
    ret = -3;    
  }
  
  if((returnBuffer[0] & 0x08) == 0x08)
  {
    std::cout << "Command timeout on motor " << index << std::endl;
    ret = -4;    
  }
  
  if((returnBuffer[0] & 0x10) == 0x10)
  {
    std::cout << "Kill switch on motor " << index << std::endl;
    ret = -5;
  }
  
  if((returnBuffer[0] & 0x20) == 0x20)
  {
    std::cout << "Low VIN on motor " << index << std::endl;
    ret = -6;
  }
  
  if((returnBuffer[0] & 0x40) == 0x40)
  {
    std::cout << "High VIN on motor " << index << std::endl;
    ret = -7;
  }
  
  if((returnBuffer[0] & 0x80) == 0x80)
  {
    std::cout << "Over temperature on motor " << index << std::endl;
    ret = -8;
  }
  
  if((returnBuffer[1] & 0x01) == 0x01)
  {
    std::cout << "Motor driver error on motor " << index << std::endl;
    ret = -9;
  }
  
  if((returnBuffer[1] & 0x02) == 0x02)
  {
    std::cout << "Err line high on motor " << index << std::endl;
    ret = -10;
  }
  
  return ret;
}

void Motor::motorForward(int index, double percent)
{
/*
  unsigned char buffer[5] = {0xAA,index,0x05,0,0};
  
  double maxPercent = MAX_VOLTAGE/24.0;
  
  buffer[3] = ((int)(percent*maxPercent*3200)) % 32;
  buffer[4] = ((int)(percent*maxPercent*3200)) / 32;  
  
  ftdi->writeData(buffer, 5);*/
double maxPercent = MAX_VOLTAGE/24.0;
  unsigned char buffer[3] = {0xFF,index,(int)(percent*maxPercent*127)+127};
  ftdi->writeData(buffer,3);
}

void Motor::motorReverse(int index, double percent)
{
/*  unsigned char buffer[5] = {0xAA,index,0x06,0,0};
  
  double maxPercent = MAX_VOLTAGE/24.0;
  
  buffer[3] = ((int)(percent*maxPercent*3200)) % 32;
  buffer[4] = ((int)(percent*maxPercent*3200)) / 32;  
  
  ftdi->writeData(buffer, 5);*/
  double maxPercent = MAX_VOLTAGE/24.0;
  unsigned char buffer[3] = {0xFF,index,(int)(-1*percent*maxPercent*127)+127};
  ftdi->writeData(buffer,3);
}

void Motor::motorBrake(int index,double percent)
{
  unsigned char buffer[4] = {0xAA,index,0x12,0};
  
  buffer[3] = ((int)(percent*32));
  
  ftdi->writeData(buffer, 5);
}

double Motor::getSpeed(int index)
{
  unsigned char buffer[4] = {0xAA,index,0x21,21};
  
  ftdi->writeData(buffer,4);
  
  unsigned char returnBuffer[2] = {0,0};
  
  ftdi->readData(&returnBuffer[0],2);
  
  int speed = returnBuffer[0] + returnBuffer[1]*256; //returns low byte first
  
  if(speed > 32767)
    speed -= 65535;
  
  double percentPower = speed/3200.;
  return percentPower;
}

double Motor::getTemperature(int index)
{
  unsigned char buffer[4] = {0xAA,index,0x21,24};
  
  ftdi->writeData(buffer,4);
  
  unsigned char returnBuffer[2] = {0,0};
  
  ftdi->readData(&returnBuffer[0],2);
  
  int rawTemp = returnBuffer[0] + returnBuffer[1]*256; //returns low byte first
  
  double temperature = .1*rawTemp;
  
  return temperature;
}

double Motor::getVoltage(int index)
{
  unsigned char buffer[4] = {0xAA,index,0x21,23};
  
  ftdi->writeData(buffer,4);
  
  unsigned char returnBuffer[2] = {0,0};
  
  ftdi->readData(&returnBuffer[0],2);
  
  int rawVolt = returnBuffer[0] + returnBuffer[1]*256; //returns low byte first
  
  double voltage = rawVolt/1000.;
  
  return voltage;
}
